/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_KINEMATICSENSOR_H_
#define __MMM_KINEMATICSENSOR_H_

#include "../../InterpolatableSensor.h"
#include "../../SensorMeasurement.h"
#include "../../../RapidXML/RapidXMLReader.h"
#include "../../../RapidXML/RapidXMLWriter.h"
#include "KinematicSensorMeasurement.h"
#include "../../../XMLTools.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include <string>
#include <vector>
#include <map>
#include <set>
#include <iostream>

namespace MMM
{

class KinematicSensor;

typedef boost::shared_ptr<KinematicSensor> KinematicSensorPtr;
typedef std::vector<KinematicSensorPtr> KinematicSensorList;

class KinematicSensor : public InterpolatableSensor<KinematicSensorMeasurement>
{

public:

    static KinematicSensorPtr loadSensorXML(RapidXMLReaderNodePtr node);

    KinematicSensor(const std::vector<std::string> &jointNames, const std::string &description = std::string());

    bool checkModel(ModelPtr model);

    boost::shared_ptr<BasicSensor<KinematicSensorMeasurement> > cloneConfiguration();

    bool equalsConfiguration(SensorPtr other);

    std::string getType();

    std::string getVersion();

    int getPriority();

    std::vector<std::string> getJointNames();

    bool addSensorMeasurement(KinematicSensorMeasurementPtr measurement);   

    static constexpr const char* TYPE = "Kinematic";

    static constexpr const char* VERSION = "1.0";

    /*! Joins all kinematic sensors together on the timestep measurements of the biggest sensor if possible for all given sensors via linear interpolation. */
    static KinematicSensorPtr join(std::vector<KinematicSensorPtr> sensors);

    /*! Joins all kinematic sensors together the given timestep if possible via linear interpolation. */
    static KinematicSensorPtr join(std::vector<KinematicSensorPtr> sensors, const std::vector<float> &timesteps);

private:

    KinematicSensor();

    void loadConfigurationXML(RapidXMLReaderNodePtr node);

    void loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type);

    void appendConfigurationXML(RapidXMLWriterNodePtr node);

    bool hasSensorConfigurationContent() const;

    std::vector<std::string> jointNames;
};
}
#endif // __MMM_KINEMATICSENSOR_H_

